//
// Created by lqk on 2022/2/5.
//

#include "OrientationEstimator.h"

void OrientationEstimator::Update() {

}

void OrientationEstimator::CheaterUpdate() {
    m_estimatedState->m_quat=g_cheaterInfo.m_quat;
    m_estimatedState->m_rotation=m_estimatedState->m_quat.toRotationMatrix();
    auto quat=m_estimatedState->m_quat.coeffs();
    m_estimatedState->m_rpy=quatToRPY(Eigen::Matrix<float,4,1>(quat[3],quat[0],quat[1],quat[2]));

//    std::cout<<m_oriState.m_rpy.transpose()<<std::endl;
//    std::cout<<m_oriState.m_rotation<<std::endl;
}
